package com.google.android.apps.skipbot;

import java.security.InvalidParameterException;

public class SkipperIKCalculator {

  private final SkipperRobotConfigInterface robotConfig;

  public SkipperIKCalculator(SkipperRobotConfigInterface robotConfig) {
    this.robotConfig = robotConfig;
  }

  /**
   * Gets three angles for the position of the leg based on an xyz position. The
   * first angle is the angle of the hip with a right hand winding when looking
   * from above. 90 degrees is straight out from the center of the robot. The
   * second angle is the angle of the shoulder measured from vertical down to
   * the upper arm. The third angle is the angle of the elbow measured between
   * the upper and lower arm.
   * 
   * @param legIndex zero based index of the leg
   * @param endPosition position in xyz spave of the end of the leg relative to
   *        the bottom center of the robot
   */
  public int[] getLegAngles(int legIndex, double[] endPosition) {
    assert (endPosition.length == 3);
    int[] angles = new int[3];

    boolean isRightHanded = robotConfig.isLegOnRightOfHip(legIndex);

    // This is the position of the hip servo of the leg in absolute space.
    double[] hipPosition = robotConfig.getHipPosition(legIndex);

    double hipLengthXY = magnitudeXY(hipPosition);

    double[] hipUnitDirectionXY = {hipPosition[0] / hipLengthXY, hipPosition[1] / hipLengthXY};

    double[] endToHipXY = subtractXY(hipPosition, endPosition);

    double[] hipToShoulderOffset = robotConfig.getHipToShoulderOffset(legIndex);
    double hipToShoulderXLength = Math.abs(hipToShoulderOffset[0]);

    double denom = endToHipXY[0] * endToHipXY[0] + endToHipXY[1] * endToHipXY[1];
    double squareRoot =
        endToHipXY[1]
            * Math.sqrt(endToHipXY[0] * endToHipXY[0] + endToHipXY[1] * endToHipXY[1]
                - hipToShoulderXLength * hipToShoulderXLength);
    double acosPlus = Math.acos((-hipToShoulderXLength * endToHipXY[0] + squareRoot) / denom);
    double acosMinus = Math.acos((-hipToShoulderXLength * endToHipXY[0] - squareRoot) / denom);
    double[] tSolutions = {acosPlus, -1.0 * acosPlus, acosMinus, -1.0 * acosMinus};

    double[][] tSolutionsXY = new double[4][2];
    double[][] a = new double[4][2];
    double[][] b = new double[4][2];
    double[][] aNorm = new double[4][2];
    double[][] bNorm = new double[4][2];
    double[] axb = new double[4];
    boolean[] test = new boolean[4];

    int solutionIndex = -1;

    for (int i = 0; i < 4; ++i) {
      tSolutionsXY[i][0] = endToHipXY[0] + hipToShoulderXLength * Math.cos(tSolutions[i]);
      tSolutionsXY[i][1] = endToHipXY[1] + hipToShoulderXLength * Math.sin(tSolutions[i]);

      a[i][0] = -1.0 * tSolutionsXY[i][0];
      a[i][1] = -1.0 * tSolutionsXY[i][1];

      b[i][0] = endToHipXY[0] - tSolutionsXY[i][0];
      b[i][1] = endToHipXY[1] - tSolutionsXY[i][1];

      double aInverseLength = 1 / magnitudeXY(a[i]);
      aNorm[i][0] = a[i][0] * aInverseLength;
      aNorm[i][1] = a[i][1] * aInverseLength;

      double bInverseLength = 1 / magnitudeXY(b[i]);
      bNorm[i][0] = b[i][0] * bInverseLength;
      bNorm[i][1] = b[i][1] * bInverseLength;

      axb[i] = aNorm[i][0] * bNorm[i][1] - aNorm[i][1] * bNorm[i][0];
      double direction = isRightHanded ? 1.0 : -1.0;
      test[i] = near(axb[i] - direction, 0.0);
      if (test[i]) {
        solutionIndex = i;
        break;
      }
    }

    if (solutionIndex == -1) {
      throw new InvalidParameterException("Solution not found on tangent circle.");
    }

    double theta =
        Math.acos((hipUnitDirectionXY[0] * bNorm[solutionIndex][0])
            + (hipUnitDirectionXY[1] * bNorm[solutionIndex][1]));

    double crossCheck =
        (hipUnitDirectionXY[0] * bNorm[solutionIndex][1])
            - (hipUnitDirectionXY[1] * bNorm[solutionIndex][0]);

    double fixTheta = theta;

    if (isRightHanded) {
      fixTheta = crossCheck > 0 ? theta : 2 * Math.PI - theta;
    } else {
      fixTheta = crossCheck < 0 ? Math.PI - theta : theta - Math.PI;
    }

    angles[0] = (int) Math.round(fixTheta * 180.0 / Math.PI);

    double[] shoulderPoint =
        {
            (hipToShoulderOffset[0] * -1.0 * hipUnitDirectionXY[0] + hipToShoulderOffset[1]
                * hipUnitDirectionXY[1])
                * Math.cos(fixTheta)
                - (hipToShoulderOffset[0] * -1.0 * hipUnitDirectionXY[1] + hipToShoulderOffset[1]
                    * -1.0 * hipUnitDirectionXY[0]) * Math.sin(fixTheta) + hipPosition[0],
            (hipToShoulderOffset[0] * -1.0 * hipUnitDirectionXY[0] + hipToShoulderOffset[1]
                * hipUnitDirectionXY[1])
                * Math.sin(fixTheta)
                + (hipToShoulderOffset[0] * -1.0 * hipUnitDirectionXY[1] + hipToShoulderOffset[1]
                    * -1.0 * hipUnitDirectionXY[0]) * Math.cos(fixTheta) + hipPosition[1],
            hipPosition[2]};

    double[] shoulderToEnd = subtractXYZ(endPosition, shoulderPoint);

    double shoulderToEndLength = magnitudeXYZ(shoulderToEnd);
    double shoulderToEndLengthSquared = shoulderToEndLength * shoulderToEndLength;
    double shoulderToEndLengthXY = magnitudeXY(shoulderToEnd);
    double upperArmLength = robotConfig.getUpperArmLength(legIndex);
    double lowerArmLength = robotConfig.getLowerArmLength(legIndex);
    double upperArmLengthSquared = upperArmLength * upperArmLength;
    double lowerArmLengthSquared = lowerArmLength * lowerArmLength;

    double beta =
        Math.acos((lowerArmLengthSquared + upperArmLengthSquared - shoulderToEndLengthSquared)
            / (2 * upperArmLength * lowerArmLength));

    // Note this should be in radians.
    double legAngleOffset = robotConfig.getLegAngleOffset(legIndex);

    angles[2] = (int) Math.round((beta + legAngleOffset) * 180.0 / Math.PI);

    double angleLowerArm =
        Math.acos((upperArmLengthSquared + shoulderToEndLengthSquared - lowerArmLengthSquared)
            / (2 * upperArmLength * shoulderToEndLength));

    double angleShoulderToEndXY =
        Math.acos((shoulderToEnd[2] * shoulderToEnd[2] + shoulderToEndLengthSquared - shoulderToEndLengthXY
            * shoulderToEndLengthXY)
            / (2 * -shoulderToEnd[2] * shoulderToEndLength));

    double alpha;
    if (near(shoulderToEnd[2], 0.0)) {
      alpha = Math.PI / 2.0 - angleLowerArm;
    } else if (shoulderToEnd[2] < 0.0) {
      alpha = Math.PI - angleLowerArm - angleShoulderToEndXY;
    } else {
      alpha = angleShoulderToEndXY - angleLowerArm;
    }

    angles[1] = (int) Math.round(alpha * 180.0 / Math.PI);

    // TODO(bbrown) : Have angle sanity checks here.
    return angles;
  }

  private double magnitudeXY(double[] pointXY) {
    assert (pointXY.length > 1);
    return Math.sqrt(pointXY[0] * pointXY[0] + pointXY[1] * pointXY[1]);
  }

  private double magnitudeXYZ(double[] pointXYZ) {
    assert (pointXYZ.length > 2);
    return Math.sqrt(pointXYZ[0] * pointXYZ[0] + pointXYZ[1] * pointXYZ[1] + pointXYZ[2]
        * pointXYZ[2]);
  }

  private double[] subtractXY(double[] point1XY, double[] point2XY) {
    assert (point1XY.length > 1);
    assert (point2XY.length > 1);
    double[] subtract = {point1XY[0] - point2XY[0], point1XY[1] - point2XY[1]};
    return subtract;
  }

  private double[] subtractXYZ(double[] point1XYZ, double[] point2XYZ) {
    assert (point1XYZ.length > 2);
    assert (point2XYZ.length > 2);
    double[] subtract =
        {point1XYZ[0] - point2XYZ[0], point1XYZ[1] - point2XYZ[1], point1XYZ[2] - point2XYZ[2]};
    return subtract;
  }

  private boolean near(double testValue, double desiredValue) {
    final double EPSILON = 0.0001;
    double diff = testValue - desiredValue;
    return diff < EPSILON && diff > -1 * EPSILON;
  }
}
